import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros.parameter_descriptions
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    OpaqueFunction,
    RegisterEventHandler,
    ExecuteProcess
)
from launch_ros.actions import Node
from launch.event_handlers import OnProcessExit

def generate_launch_description():
    robot_name='aubo_i10'
    urdf_tutorial_path = get_package_share_directory('Robo_assembly_description')
    default_model_path = urdf_tutorial_path + '/urdf/aubo_i10.urdf.xacro'
    default_world_path = get_package_share_directory('Robo_assemble_gz') + '/config/empty.sdf'
    action_declare_arg_mode_path=DeclareLaunchArgument(
        name='model',default_value=default_model_path,description='Absolute path to robot urdf file')
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]
        ),value_type=str
    )
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}],
    )


    launch_gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros') + '/launch/gazebo.launch.py']),
        launch_arguments=[('world',default_world_path),('verbose','true')]
    )

    spawn_entity=launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity',robot_name,'-topic','/robot_description'],
    )

    # load_joint_state_controller=ExecuteProcess(
    #     cmd=['ros2','control','load_controller','joint_state_broadcaster','--set-state','active'],
    #     output='screen'
    # )
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "joint_trajectory_controller","forward_command_controller_position","--controller-manager", "/controller_manager"],
    )
    # delay_entity_after_joint_state_controller = RegisterEventHandler(
    #     event_handler=OnProcessExit(
    #         target_action=spawn_entity,
    #         on_exit=[load_joint_state_controller],
    #     )
    # )
    delay_entity_after_joint_state_controller = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=spawn_entity,
            on_exit=[joint_state_broadcaster_spawner],
        )
    ) 
        
    nodes_to_start=launch.LaunchDescription(
        [action_declare_arg_mode_path,
         robot_state_publisher_node,
         launch_gazebo,
         spawn_entity,
        #  delay_entity_after_joint_state_controller
        # load_joint_state_controller
        # joint_state_broadcaster_spawner
        ]
        )

    return nodes_to_start
